//
// Created by fazhehy on 2024/6/8.
//

#include "task_indoor_precise.h"

servo_param_t indoor_precise_sound;
uint8_t indoor_precise_config(uint8_t keyNum)
{
    static uint8_t times = 0, pos = 0, isSelected = 0;
    static uint16_t x = 0, y = 0, len = 0;

    if (times == 0){
        pos = 0;
        isSelected = 0;
        x = DISPLAY_OFFSET_X;
        y = DISPLAY_OFFSET_Y+pos*20;
        len = 11*FONT_WIDTH+ROUND_RECT_R;
        snprintf(str[0], 64, "precise run");
        snprintf(str[1], 64, "precise.speed  ----->%5d", indoor_precise_sound.speed);
        snprintf(str[2], 64, "precise.servo.p----->%6.3f", indoor_precise_sound.p);
        snprintf(str[3], 64, "precise.servo.i----->%6.3f", indoor_precise_sound.i);
        snprintf(str[4], 64, "precise.servo.d----->%6.3f", indoor_precise_sound.d);
        for (int i = 0; i < 5; ++i) {
            st7789_show_string(2, i * 20 + DISPLAY_OFFSET_Y + 2, str[i]);
        }
        times = 1;
        *p_indoor_flag = true;
        SCB_CleanInvalidateDCache();
    }

    if (keyNum != NOP && isSelected == 0)
        st7789_draw_round_rectangle(x, y, len, FONT_HEIGHT+4, ROUND_RECT_R, ST7789_BG_COLOR);

    if (keyNum == UP || keyNum & ENCODER_POS_PREFIX){
        if (isSelected == 0){
            if (pos != 0)
                pos --;
        }
        else{
            switch (pos) {
                case 1: indoor_precise_sound.speed += 10;break;
                case 2: indoor_precise_sound.p += 1.0f;break;
                case 3: indoor_precise_sound.i += 0;break;
                case 4: indoor_precise_sound.d += 1.0f;break;
                default:break;
            }
        }
    }
    else if (keyNum == DOWN || keyNum & ENCODER_NEG_PREFIX){
        if (isSelected == 0){
            if (pos != 4)
                pos ++;
        }
        else{
            switch (pos) {
                case 1: indoor_precise_sound.speed -= 10;break;
                case 2: indoor_precise_sound.p -= 1.0f;break;
                case 3: indoor_precise_sound.i -= 0;break;
                case 4: indoor_precise_sound.d -= 1.0f;break;
                default:break;
            }
        }
    }
    else if (keyNum == ENTER){
        if (pos != 0){
            if (isSelected == 1)
                isSelected = 0;
            else{
                isSelected = 1;
                x = DISPLAY_OFFSET_X;
                y = DISPLAY_OFFSET_Y+pos*20;
                len = strlen(str[pos])*FONT_WIDTH+ROUND_RECT_R;

                st7789_draw_round_rectangle(x, y, len, FONT_HEIGHT+4, ROUND_RECT_R, ST7789_PEN_COLOR);
            }
        }
        else{
            times = 0;
//            flash_write_parameter();
            return ENTER;
        }
    }
    else if (keyNum == BACK){
//        flash_write_parameter();
        times = 0;
        return BACK;
    }

    st7789_printf(40, 20, "isSelected:%d", isSelected);
    if (isSelected == 1){
        switch (pos) {
            case 1:snprintf(str[1], 64, "precise.speed  ----->%5d",   indoor_precise_sound.speed);break;
            case 2:snprintf(str[2], 64, "precise.servo.p----->%6.3f", indoor_precise_sound.p);break;
            case 3:snprintf(str[3], 64, "precise.servo.i----->%6.3f", indoor_precise_sound.i);break;
            case 4:snprintf(str[4], 64, "precise.servo.d----->%6.3f", indoor_precise_sound.d);break;
            default:
                break;
        }
        st7789_show_string(2, pos * 20 + DISPLAY_OFFSET_Y + 2, str[pos]);
    }

    if (keyNum != NOP && isSelected == 0){
        x = DISPLAY_OFFSET_X;
        y = DISPLAY_OFFSET_Y+pos*20;
        len = strlen(str[pos])*FONT_WIDTH+ROUND_RECT_R;

        st7789_draw_round_rectangle(x, y, len, FONT_HEIGHT+4, ROUND_RECT_R, ST7789_PEN_COLOR);
    }

    return NO_SHOW;
}

uint8_t indoor_precise_run(uint8_t keyNum)
{
    float target_angle = 0;

    uint8_t now_num = 0;
    uint8_t last_num = 0;

    int16_t duty = 0;

    servo_init();
    position_pid_init(&servo_pid, -indoor_precise_sound.p, -indoor_precise_sound.i, -indoor_precise_sound.d, SERVO_LIMIT, 300);
    servo_set_duty(SERVO_MEDIAN);

    while (lora_receive_num == 0){
        st7789_printf(40, 0, "Ready!!!!");
    }
    st7789_printf(40, 0, "Start!!!!");

    now_num = lora_receive_num;
    last_num = now_num;

    *p_target_bldc_duty = indoor_precise_sound.speed;
    SCB_CleanInvalidateDCache();

    while (1){
        now_num = lora_receive_num;
        if (now_num == 0)
            break;

        if (now_num != last_num){
            *p_target_bldc_duty = 0;
            SCB_CleanInvalidateDCache();
            last_num = now_num;
        }
        else{
            if (fabsf(target_angle) > 60){
                *p_target_bldc_duty =(int16_t)(400-400*0.5f*(fabsf(target_angle)/180));
            }
            else{
                *p_target_bldc_duty = (int16_t )(indoor_precise_sound.speed - ((float)indoor_precise_sound.speed*0.6)*(fabsf(target_angle)/180));
            }
            SCB_CleanInvalidateDCache();
            target_angle = (float )m7_1_sound_position[1];

            duty = (int16_t) position_pid(&servo_pid, target_angle, 0);
            servo_set_duty((int16_t)(duty + SERVO_MEDIAN));
        }

        st7789_printf(0, 40, "target_angle:%4f\nposition1:%4d\n", target_angle, m7_1_sound_position[1]);
    }

    servo_set_duty(SERVO_MEDIAN);
    *p_target_bldc_duty = 0;
    SCB_CleanInvalidateDCache();
    motor_stop();
    beep_off();

    return BACK;
}

